#include "kernel.h"

typedef
int(*mmoveproc_t)(char *dest, char *base, size_t offset, size_t fillsize, size_t copysize);

/* simple routine for loading ELF */
static int
load_elf_image(struct elf_t *image, mmoveproc_t mmove)
{
     struct proghdr_t *ph, *eph;
	 int err = 0;
	 
     if (image->e_magic != ELF_MAGIC)
		  return -E_INVAL;

     ph  = (struct proghdr_t *)((char *)image + image->e_phoff);
     eph = ph + image->e_phnum;

     for ( ; ph != eph; ++ph)
     {
		  if (ph->p_type == ELF_PROG_LOAD)
		  {
			   if ((err = mmove((char *)ph->p_va, (char *)image,
								ph->p_offset, ph->p_memsz, ph->p_filesz))
				   != 0)
			   {
					break;
			   }
		  }
     }

	 return err;
}

int
my_move(char *dest, char *base, size_t offset, size_t fillsize, size_t copysize)
{
#ifdef K_INIT_DEBUG
     kprintf(" -- trying to move [%x, %x] to [%x, %x]\n",
			 base + offset, base + offset + copysize,
			 dest, dest + fillsize);
#endif

     base = base + offset; offset = 0;

     uintptr_t r = (uintptr_t)dest & (PAGE_SIZE - 1);     
     int err;

     uint32_t perm;
     
     if (r)
     {
		  r = PAGE_SIZE - r;
		  perm = PTE_W | PTE_U;
		  if ((err = fix_addr(dest, &perm, false)) != 0)
		  {
			   return err;
		  }
		       
		  if (r > copysize)
		  {
			   memmove(dest, base, copysize);
			   memset(dest, 0, r - copysize);
	       
		  }
		  else
		  {
			   memmove(dest, base, r);
		  }
	  
		  dest += r;
		  base += r;
		  offset += r;
     }
     
     while (offset < fillsize)
     {
		  perm = PTE_W | PTE_U;
		  if ((err = fix_addr(dest, &perm, false)) != 0)
		  {
			   return err;
		  }
	  
		  if (offset + PAGE_SIZE <=  copysize)
			   memmove(dest, base, PAGE_SIZE);
		  else if (offset < copysize)
		  {
			   memmove(dest, base, copysize - offset);
			   memset(dest + copysize - offset, 0, PAGE_SIZE - copysize + offset);
		  }
		  else memset(dest, 0, PAGE_SIZE);

		  dest += PAGE_SIZE;
		  base += PAGE_SIZE;
		  offset += PAGE_SIZE;
     }

     return 0;
}
		  
void
kern_main()
{
	 /* Initialize the system kernel */
	 /* clear the data in the bss segment */
	 memset(_bss, 0, _end - _bss);

	 io_init();
	 io_console_enabled = 1;

	 kprintf("%s %s initailizing\n", EKOS_NAME, EKOS_VERSION);

	 const char *subsys;
	 int errno;
	 subsys = "memory";
	 kprintf("%s: ", subsys);
	 if ((errno = mem_init()) < 0)
		  goto error;
	 else kprintf("OK.\n");

	 subsys = "sysconf";
	 kprintf("%s: ", subsys);
	 if ((errno = sys_init()) < 0)
	 	  goto error;
	 else kprintf("OK.\n");

	 subsys = "interruption";
	 kprintf("%s: ", subsys);
	 if ((errno = intr_init()) < 0)
		  goto error;
	 else kprintf("OK.\n");

	 if (sysconf.has_lapic)
	 {
		  subsys = "lapic";
		  kprintf("%s: ", subsys);
		  if ((errno = lapic_init()) < 0)
			   goto error;
		  else kprintf("OK.\n");
		  sysconf.use_lapic = 1;
	 }

	 subsys = "process";
	 kprintf("%s: ", subsys);
	 if ((errno = proc_init()) < 0)
		  goto error;
	 else kprintf("OK.\n");

	 subsys = "taskgate";
	 kprintf("%s: ", subsys);
	 if ((errno = task_init()) < 0)
		  goto error;
	 else kprintf("OK.\n");
	 
	 kprintf("\nFinished, now begins to load user components.\n");

	 /* load elf */
	 if ((errno = load_elf_image((struct elf_t *)user_lib, my_move)) != 0)
	 {
		  kprintf("Failed\n");
		  readline("System shutdown, press enter to reboot\n");
		  reboot();
	 }
	 
	 kprintf("Jump into userland\n");
	 /* Now we would enable the kernel log pipe for kernel logs */
	 // io_log_enabled = 1;
	 /* You want the console to be silent? */
	 // io_console_enabled = 0;
	 jump_user((void *)((struct elf_t *)user_lib)->e_entry, (void *)0,
			   read_eflags() | FL_IF | FL_IOPL_3);
	 
	 while (1) ;

error:
	 kprintf("error while initializing %s subsystem: %d\n", subsys, errno);
	 readline("System shutdown, press enter to reboot\n");
	 reboot();
}
